#include "constants.h"

#include "MyPose.h"
#include "Robot.h"
#include "LNP.h"
#include "OdomMove.h"
#include "MotionModel.h"
#include "SensorModel.h"
#include "MapServer.h"
#include "PoseRange.h"
#include "PoseArrayVisualizer.h"
#include "ParticleFilter.h"
#include "InitialPoses.h"

#ifndef LOCALIZER_H_
#define LOCALIZER_H_

class Localizer{

public:
	Localizer(ros::NodeHandle &n);
	virtual ~Localizer();

	/** Independent threads **/
	void publishMapOdomTransform();
	virtual void readOdom(nav_msgs::Odometry msg);

protected:
	double initialEstimateProvided;
	int itrParticleFilter;
	Robot config;
	LNP obstaclecost;
	InitialPoses PoseCreator;
	MotionModel mm;
    MapServer ms;
    SensorModel sm;

    ParticleFilter pf;

	PoseArray pa;
	WeightsForPoses w;
    OdomReading last;
	PoseArrayVisualizer paviz;
    OdomMove om;

    CostMarker cm;
/** Other ros objects ***/
	ros::Subscriber subo, subi;
	ros::Publisher cost_marker_pub;
	ros::Duration transform_tolerance_;

	tf::StampedTransform transform;
    tf::TransformListener listener;
    tf::TransformBroadcaster br;
    tf::Transform map_to_odom;

	MATRIX map;

	bool getMap();
	bool tooMuchRotation(OdomReading &last,OdomReading &current);
	bool compareCurrentOdomReadingToNew(OdomReading &last,OdomReading &current);
	virtual void localize(OdomReading &last, OdomReading &current);
	void botPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
	void makeMapOdomtransform(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
};

#endif /* LOCALIZER_H_ */
